<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<html xmlns="http://www.w3.org/1999/xhtml">
<head>
<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
<title>GSoC2011SfM: D:/Travail/These/Determination caracteristiques camera/GSoC/SfM/src/bundle_related.h Source File</title>

<link href="tabs.css" rel="stylesheet" type="text/css"/>
<link href="doxygen.css" rel="stylesheet" type="text/css" />

<link href="search/search.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="jquery.js"></script>
<script type="text/javascript" src="search/search.js"></script>
<script type="text/javascript">
  $(document).ready(function() { searchBox.OnSelectItem(0); });
</script>

</head>
<body>
<div id="top"><!-- do not remove this div! -->


<div id="titlearea">
<table cellspacing="0" cellpadding="0">
 <tbody>
 <tr style="height: 56px;">
  
  <td id="projectlogo"><img alt="Logo" src="logo.png"/></td>
  
  
  <td style="padding-left: 0.5em;">
   <div id="projectname">GSoC2011SfM
   &#160;<span id="projectnumber">0.1</span>
   </div>
   <div id="projectbrief">Google Summer of Code 2011: Structure from motion</div>
  </td>
  
  
  
 </tr>
 </tbody>
</table>
</div>

<!-- Generated by Doxygen 1.7.5.1 -->
<script type="text/javascript">
var searchBox = new SearchBox("searchBox", "search",false,'Search');
</script>
  <div id="navrow1" class="tabs">
    <ul class="tablist">
      <li><a href="index.html"><span>Main&#160;Page</span></a></li>
      <li><a href="annotated.html"><span>Classes</span></a></li>
      <li class="current"><a href="files.html"><span>Files</span></a></li>
      <li>
        <div id="MSearchBox" class="MSearchBoxInactive">
        <span class="left">
          <img id="MSearchSelect" src="search/mag_sel.png"
               onmouseover="return searchBox.OnSearchSelectShow()"
               onmouseout="return searchBox.OnSearchSelectHide()"
               alt=""/>
          <input type="text" id="MSearchField" value="Search" accesskey="S"
               onfocus="searchBox.OnSearchFieldFocus(true)" 
               onblur="searchBox.OnSearchFieldFocus(false)" 
               onkeyup="searchBox.OnSearchFieldChange(event)"/>
          </span><span class="right">
            <a id="MSearchClose" href="javascript:searchBox.CloseResultsWindow()"><img id="MSearchCloseImg" border="0" src="search/close.png" alt=""/></a>
          </span>
        </div>
      </li>
    </ul>
  </div>
  <div id="navrow2" class="tabs2">
    <ul class="tablist">
      <li><a href="files.html"><span>File&#160;List</span></a></li>
    </ul>
  </div>
<div class="header">
  <div class="headertitle">
<div class="title">D:/Travail/These/Determination caracteristiques camera/GSoC/SfM/src/bundle_related.h</div>  </div>
</div>
<div class="contents">
<div class="fragment"><pre class="fragment"><a name="l00001"></a>00001 <span class="preprocessor">#ifndef _GSOC_SFM_POINTS_BUNDLE_RELATED_H</span>
<a name="l00002"></a>00002 <span class="preprocessor"></span><span class="preprocessor">#define _GSOC_SFM_POINTS_BUNDLE_RELATED_H 1</span>
<a name="l00003"></a>00003 <span class="preprocessor"></span>
<a name="l00004"></a>00004 <span class="preprocessor">#include &lt;sba.h&gt;</span>
<a name="l00005"></a>00005 <span class="preprocessor">#include &quot;libmv/numeric/numeric.h&quot;</span>
<a name="l00006"></a>00006 
<a name="l00007"></a>00007 <span class="comment">//A lot of these functions are extracted from Lourakis&#39; SBA:</span>
<a name="l00008"></a>00008 <span class="comment">//http://www.ics.forth.gr/~lourakis/sba/</span>
<a name="l00009"></a>00009 
<a name="l00010"></a>00010 <span class="keyword">namespace </span>OpencvSfM{
<a name="l00011"></a>00011 
<a name="l00012"></a>00012 
<a name="l00016"></a><a class="code" href="struct_opencv_sf_m_1_1bundle__datas.html">00016</a> <span class="keyword">struct </span><a class="code" href="struct_opencv_sf_m_1_1bundle__datas.html">bundle_datas</a>
<a name="l00017"></a>00017 {
<a name="l00018"></a><a class="code" href="struct_opencv_sf_m_1_1bundle__datas.html#a538670d55815b8edadc2716ea4d3afe5">00018</a>   libmv::vector&lt; libmv::Mat3 &gt;&amp; <a class="code" href="struct_opencv_sf_m_1_1bundle__datas.html#a538670d55815b8edadc2716ea4d3afe5" title="list of intra parameters of each cameras">intraParams</a>;
<a name="l00019"></a><a class="code" href="struct_opencv_sf_m_1_1bundle__datas.html#abe6eba2c3f3f989df9330c4814a7cb03">00019</a>   libmv::vector&lt; Eigen::Quaterniond &gt;&amp; <a class="code" href="struct_opencv_sf_m_1_1bundle__datas.html#abe6eba2c3f3f989df9330c4814a7cb03" title="list of rotations matrix of each cameras">rotations</a>;
<a name="l00020"></a><a class="code" href="struct_opencv_sf_m_1_1bundle__datas.html#a825755e13d5cd58e7a5ccc04e63d0ed0">00020</a>   libmv::vector&lt; libmv::Vec3 &gt;&amp; <a class="code" href="struct_opencv_sf_m_1_1bundle__datas.html#a825755e13d5cd58e7a5ccc04e63d0ed0" title="list of translation vector of each cameras">translations</a>;
<a name="l00021"></a><a class="code" href="struct_opencv_sf_m_1_1bundle__datas.html#a842e05defa44648188b6cc2a7bb64b95">00021</a>   <span class="keywordtype">double</span>* <a class="code" href="struct_opencv_sf_m_1_1bundle__datas.html#a842e05defa44648188b6cc2a7bb64b95" title="List of 3d points.">points3D</a>;
<a name="l00022"></a><a class="code" href="struct_opencv_sf_m_1_1bundle__datas.html#af15cb0e007387a64a3fdc551f3f84b0c">00022</a>   <span class="keywordtype">int</span> <a class="code" href="struct_opencv_sf_m_1_1bundle__datas.html#af15cb0e007387a64a3fdc551f3f84b0c" title="number of parameters for ONE camera; e.g. 6 for Euclidean cameras">cnp</a>;
<a name="l00023"></a><a class="code" href="struct_opencv_sf_m_1_1bundle__datas.html#a15942e3b23aafd46b44ece46e49985f1">00023</a>   <span class="keywordtype">int</span> <a class="code" href="struct_opencv_sf_m_1_1bundle__datas.html#a15942e3b23aafd46b44ece46e49985f1" title="number of parameters for ONE 3D point; e.g. 3 for Euclidean points">pnp</a>;
<a name="l00024"></a><a class="code" href="struct_opencv_sf_m_1_1bundle__datas.html#a86083a2e4a4fd435d517be6283d4f347">00024</a>   <span class="keywordtype">int</span> <a class="code" href="struct_opencv_sf_m_1_1bundle__datas.html#a86083a2e4a4fd435d517be6283d4f347" title="number of parameters for ONE projected point; e.g. 2 for Euclidean points">mnp</a>;
<a name="l00025"></a><a class="code" href="struct_opencv_sf_m_1_1bundle__datas.html#aa2a4b3786215234687f39695d141b207">00025</a>   <span class="keywordtype">int</span> <a class="code" href="struct_opencv_sf_m_1_1bundle__datas.html#aa2a4b3786215234687f39695d141b207" title="number of points (starting from the 1st) whose parameters should not be modified.">ncon</a>;
<a name="l00026"></a><a class="code" href="struct_opencv_sf_m_1_1bundle__datas.html#a93d03e7e8d2fc5b2b055830c3821ad55">00026</a>   <span class="keywordtype">int</span> <a class="code" href="struct_opencv_sf_m_1_1bundle__datas.html#a93d03e7e8d2fc5b2b055830c3821ad55" title="number of cameras (starting from the 1st) whose parameters should not be modified.">mcon</a>;
<a name="l00027"></a>00027 
<a name="l00039"></a><a class="code" href="struct_opencv_sf_m_1_1bundle__datas.html#a7a729d6408ddf46eb4722a4e7a90e5f2">00039</a>   <a class="code" href="struct_opencv_sf_m_1_1bundle__datas.html#a7a729d6408ddf46eb4722a4e7a90e5f2">bundle_datas</a>(libmv::vector&lt; libmv::Mat3 &gt;&amp; i,
<a name="l00040"></a>00040     libmv::vector&lt; Eigen::Quaterniond &gt;&amp; r,
<a name="l00041"></a>00041     libmv::vector&lt; libmv::Vec3 &gt;&amp; t,
<a name="l00042"></a>00042     <span class="keywordtype">int</span> c, <span class="keywordtype">int</span> p, <span class="keywordtype">int</span> mp, <span class="keywordtype">int</span> n, <span class="keywordtype">int</span> m)
<a name="l00043"></a>00043     :<a class="code" href="struct_opencv_sf_m_1_1bundle__datas.html#a538670d55815b8edadc2716ea4d3afe5" title="list of intra parameters of each cameras">intraParams</a>(i),<a class="code" href="struct_opencv_sf_m_1_1bundle__datas.html#abe6eba2c3f3f989df9330c4814a7cb03" title="list of rotations matrix of each cameras">rotations</a>(r),<a class="code" href="struct_opencv_sf_m_1_1bundle__datas.html#a825755e13d5cd58e7a5ccc04e63d0ed0" title="list of translation vector of each cameras">translations</a>(t),<a class="code" href="struct_opencv_sf_m_1_1bundle__datas.html#af15cb0e007387a64a3fdc551f3f84b0c" title="number of parameters for ONE camera; e.g. 6 for Euclidean cameras">cnp</a>(c), <a class="code" href="struct_opencv_sf_m_1_1bundle__datas.html#a15942e3b23aafd46b44ece46e49985f1" title="number of parameters for ONE 3D point; e.g. 3 for Euclidean points">pnp</a>(p),<a class="code" href="struct_opencv_sf_m_1_1bundle__datas.html#a86083a2e4a4fd435d517be6283d4f347" title="number of parameters for ONE projected point; e.g. 2 for Euclidean points">mnp</a>(mp), <a class="code" href="struct_opencv_sf_m_1_1bundle__datas.html#a93d03e7e8d2fc5b2b055830c3821ad55" title="number of cameras (starting from the 1st) whose parameters should not be modified.">mcon</a>(m),<a class="code" href="struct_opencv_sf_m_1_1bundle__datas.html#aa2a4b3786215234687f39695d141b207" title="number of points (starting from the 1st) whose parameters should not be modified.">ncon</a>(n)
<a name="l00044"></a>00044   {
<a name="l00045"></a>00045     <a class="code" href="struct_opencv_sf_m_1_1bundle__datas.html#a842e05defa44648188b6cc2a7bb64b95" title="List of 3d points.">points3D</a> = NULL;
<a name="l00046"></a>00046   }
<a name="l00047"></a>00047 };
<a name="l00048"></a>00048 
<a name="l00049"></a>00049 
<a name="l00050"></a>00050 <span class="keywordtype">void</span> calcImgProjFullR(<span class="keywordtype">double</span> a[5],<span class="keywordtype">double</span> qr0[4],<span class="keywordtype">double</span> t[3],<span class="keywordtype">double</span> M[3],
<a name="l00051"></a>00051   <span class="keywordtype">double</span> n[2])
<a name="l00052"></a>00052 {
<a name="l00053"></a>00053   <span class="keywordtype">double</span> t1;
<a name="l00054"></a>00054   <span class="keywordtype">double</span> t11;
<a name="l00055"></a>00055   <span class="keywordtype">double</span> t13;
<a name="l00056"></a>00056   <span class="keywordtype">double</span> t17;
<a name="l00057"></a>00057   <span class="keywordtype">double</span> t2;
<a name="l00058"></a>00058   <span class="keywordtype">double</span> t22;
<a name="l00059"></a>00059   <span class="keywordtype">double</span> t27;
<a name="l00060"></a>00060   <span class="keywordtype">double</span> t3;
<a name="l00061"></a>00061   <span class="keywordtype">double</span> t38;
<a name="l00062"></a>00062   <span class="keywordtype">double</span> t46;
<a name="l00063"></a>00063   <span class="keywordtype">double</span> t49;
<a name="l00064"></a>00064   <span class="keywordtype">double</span> t5;
<a name="l00065"></a>00065   <span class="keywordtype">double</span> t6;
<a name="l00066"></a>00066   <span class="keywordtype">double</span> t8;
<a name="l00067"></a>00067   <span class="keywordtype">double</span> t9;
<a name="l00068"></a>00068   {
<a name="l00069"></a>00069     t1 = a[0];
<a name="l00070"></a>00070     t2 = qr0[1];
<a name="l00071"></a>00071     t3 = M[0];
<a name="l00072"></a>00072     t5 = qr0[2];
<a name="l00073"></a>00073     t6 = M[1];
<a name="l00074"></a>00074     t8 = qr0[3];
<a name="l00075"></a>00075     t9 = M[2];
<a name="l00076"></a>00076     t11 = -t3*t2-t5*t6-t8*t9;
<a name="l00077"></a>00077     t13 = qr0[0];
<a name="l00078"></a>00078     t17 = t13*t3+t5*t9-t8*t6;
<a name="l00079"></a>00079     t22 = t6*t13+t8*t3-t9*t2;
<a name="l00080"></a>00080     t27 = t13*t9+t6*t2-t5*t3;
<a name="l00081"></a>00081     t38 = -t5*t11+t13*t22-t27*t2+t8*t17+t[1];
<a name="l00082"></a>00082     t46 = -t11*t8+t13*t27-t5*t17+t2*t22+t[2];
<a name="l00083"></a>00083     t49 = 1/t46;
<a name="l00084"></a>00084     n[0] = (t1*(-t2*t11+t13*t17-t22*t8+t5*t27+t[0])+a[4]*t38+a[1]*t46)*t49;
<a name="l00085"></a>00085     n[1] = (t1*a[3]*t38+a[2]*t46)*t49;
<a name="l00086"></a>00086     <span class="keywordflow">return</span>;
<a name="l00087"></a>00087   }
<a name="l00088"></a>00088 }
<a name="l00089"></a>00089 
<a name="l00090"></a>00090 
<a name="l00091"></a>00091 <span class="keyword">inline</span> <span class="keyword">static</span> <span class="keywordtype">void</span> quatMultFast(<span class="keywordtype">double</span> q1[4], <span class="keywordtype">double</span> q2[4], <span class="keywordtype">double</span> p[4])
<a name="l00092"></a>00092 {
<a name="l00093"></a>00093   <span class="keywordtype">double</span> t1, t2, t3, t4, t5, t6, t7, t8, t9;
<a name="l00094"></a>00094   <span class="comment">//double t10, t11, t12;</span>
<a name="l00095"></a>00095 
<a name="l00096"></a>00096   t1=(q1[0]+q1[1])*(q2[0]+q2[1]);
<a name="l00097"></a>00097   t2=(q1[3]-q1[2])*(q2[2]-q2[3]);
<a name="l00098"></a>00098   t3=(q1[1]-q1[0])*(q2[2]+q2[3]);
<a name="l00099"></a>00099   t4=(q1[2]+q1[3])*(q2[1]-q2[0]);
<a name="l00100"></a>00100   t5=(q1[1]+q1[3])*(q2[1]+q2[2]);
<a name="l00101"></a>00101   t6=(q1[1]-q1[3])*(q2[1]-q2[2]);
<a name="l00102"></a>00102   t7=(q1[0]+q1[2])*(q2[0]-q2[3]);
<a name="l00103"></a>00103   t8=(q1[0]-q1[2])*(q2[0]+q2[3]);
<a name="l00104"></a>00104 
<a name="l00105"></a>00105 
<a name="l00106"></a>00106   t9=0.5*(t5-t6+t7+t8);
<a name="l00107"></a>00107   p[0]= t2 + t9-t5;
<a name="l00108"></a>00108   p[1]= t1 - t9-t6;
<a name="l00109"></a>00109   p[2]=-t3 + t9-t8;
<a name="l00110"></a>00110   p[3]=-t4 + t9-t7;
<a name="l00111"></a>00111 }
<a name="l00112"></a>00112 
<a name="l00113"></a>00113 <span class="keywordtype">void</span> img_projsRTS_x(<span class="comment">/*cameras and points*/</span> <span class="keywordtype">double</span> *p,
<a name="l00114"></a>00114 <span class="comment">/*sparse matrix of 2D points*/</span> <span class="keyword">struct</span> sba_crsm *idxij,
<a name="l00115"></a>00115   <span class="comment">/*tmp vector*/</span><span class="keywordtype">int</span> *rcidxs, <span class="comment">/*tmp vector*/</span><span class="keywordtype">int</span> *rcsubs,
<a name="l00116"></a>00116   <span class="comment">/*out vect*/</span><span class="keywordtype">double</span> *hx,
<a name="l00117"></a>00117   <span class="keywordtype">void</span> *adata)
<a name="l00118"></a>00118 {
<a name="l00119"></a>00119   <span class="comment">//as we do an euclidean bundle adjustement, the adata contain constant params.</span>
<a name="l00120"></a>00120   bundle_datas* datas = ((bundle_datas*)adata);
<a name="l00121"></a>00121   <span class="keyword">register</span> <span class="keywordtype">int</span> i, j;
<a name="l00122"></a>00122   <span class="keywordtype">int</span> cnp = datas-&gt;cnp, pnp = datas-&gt;pnp, mnp = datas-&gt;mnp;
<a name="l00123"></a>00123   <span class="keywordtype">double</span> *pa, *pb, *pqr, *pt, *ppt, *pmeas, lrot[4], trot[4];
<a name="l00124"></a>00124   <span class="comment">//int n;</span>
<a name="l00125"></a>00125   <span class="keywordtype">int</span> m, nnz;
<a name="l00126"></a>00126 
<a name="l00127"></a>00127   m=idxij-&gt;nc;
<a name="l00128"></a>00128   pa=p; pb=p+m*cnp;
<a name="l00129"></a>00129 
<a name="l00130"></a>00130   <span class="keywordflow">for</span>(j=0; j&lt;m; ++j){
<a name="l00131"></a>00131     <span class="comment">/* j-th camera parameters */</span>
<a name="l00132"></a>00132     libmv::Mat3 rotation;
<a name="l00133"></a>00133     libmv::Mat3&amp; K = datas-&gt;intraParams[j];
<a name="l00134"></a>00134     Eigen::Quaterniond rot_init = datas-&gt;rotations[j];
<a name="l00135"></a>00135 
<a name="l00136"></a>00136     <span class="keywordtype">double</span> Kparms[] = {K( 0,0 ),K( 2,0 ),K( 2,1 ),K( 1,1 )/K( 0,0 ),K( 1,0 )};
<a name="l00137"></a>00137     <span class="comment">// full quat for initial rotation estimate:</span>
<a name="l00138"></a>00138     <span class="keywordtype">double</span> pr0[] = {rot_init.w(), rot_init.x(), rot_init.y(), rot_init.z()};
<a name="l00139"></a>00139     libmv::Mat34 proj;
<a name="l00140"></a>00140 
<a name="l00141"></a>00141     pqr=pa+j*cnp;
<a name="l00142"></a>00142     pt=pqr+3; <span class="comment">// quaternion vector part has 3 elements</span>
<a name="l00143"></a>00143     libmv::Vec3 translat = datas-&gt;translations[j];
<a name="l00144"></a>00144     <span class="keywordtype">double</span> trans[3] = {
<a name="l00145"></a>00145       pt[0] + translat(0),
<a name="l00146"></a>00146       pt[1] + translat(1),
<a name="l00147"></a>00147       pt[2] + translat(2)};
<a name="l00148"></a>00148       lrot[1]=pqr[0]; lrot[2]=pqr[1]; lrot[3]=pqr[2];
<a name="l00149"></a>00149       lrot[0]=(1.0 - lrot[1]*lrot[1] - lrot[2]*lrot[2]- lrot[3]*lrot[3]);
<a name="l00150"></a>00150       <span class="keywordflow">if</span>( lrot[0]&gt;0 )
<a name="l00151"></a>00151         lrot[0] = sqrt( lrot[0] );
<a name="l00152"></a>00152       <span class="keywordflow">else</span>{<span class="comment">//problem with this rotation...</span>
<a name="l00153"></a>00153         lrot[0] = 0;
<a name="l00154"></a>00154         Eigen::Quaterniond quat_delta( lrot[0], lrot[1], lrot[2], lrot[3] );
<a name="l00155"></a>00155         quat_delta.normalize();
<a name="l00156"></a>00156         lrot[1]=quat_delta.x(); lrot[2]=quat_delta.y(); lrot[3]=quat_delta.z();
<a name="l00157"></a>00157         lrot[0] = quat_delta.w();
<a name="l00158"></a>00158       }
<a name="l00159"></a>00159 
<a name="l00160"></a>00160       quatMultFast(lrot, pr0, trot); <span class="comment">// trot=lrot*pr0</span>
<a name="l00161"></a>00161 
<a name="l00162"></a>00162       nnz=sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); <span class="comment">/* find nonzero hx_ij, i=0...n-1 */</span>
<a name="l00163"></a>00163 
<a name="l00164"></a>00164       <span class="keywordflow">for</span>(i=0; i&lt;nnz; ++i){
<a name="l00165"></a>00165         ppt=pb + rcsubs[i]*pnp;
<a name="l00166"></a>00166         pmeas=hx + idxij-&gt;val[rcidxs[i]]*mnp; <span class="comment">// set pmeas to point to hx_ij</span>
<a name="l00167"></a>00167 
<a name="l00168"></a>00168         calcImgProjFullR(Kparms, trot, trans, ppt, pmeas); <span class="comment">// evaluate Q in pmeas</span>
<a name="l00169"></a>00169       }
<a name="l00170"></a>00170   }
<a name="l00171"></a>00171 }
<a name="l00172"></a>00172 <span class="keywordtype">void</span> img_projsRT_x(<span class="comment">/*cameras and points*/</span> <span class="keywordtype">double</span> *p,
<a name="l00173"></a>00173 <span class="comment">/*sparse matrix of 2D points*/</span> <span class="keyword">struct</span> sba_crsm *idxij,
<a name="l00174"></a>00174   <span class="comment">/*tmp vector*/</span><span class="keywordtype">int</span> *rcidxs, <span class="comment">/*tmp vector*/</span><span class="keywordtype">int</span> *rcsubs,
<a name="l00175"></a>00175   <span class="comment">/*out vect*/</span><span class="keywordtype">double</span> *hx,
<a name="l00176"></a>00176   <span class="keywordtype">void</span> *adata)
<a name="l00177"></a>00177 {
<a name="l00178"></a>00178   <span class="comment">//as we do an euclidean bundle adjustement, the adata contain constant params.</span>
<a name="l00179"></a>00179   bundle_datas* datas = ((bundle_datas*)adata);
<a name="l00180"></a>00180   <span class="keyword">register</span> <span class="keywordtype">int</span> i, j;
<a name="l00181"></a>00181   <span class="keywordtype">int</span> cnp = datas-&gt;cnp, pnp = datas-&gt;pnp, mnp = datas-&gt;mnp;
<a name="l00182"></a>00182   <span class="keywordtype">double</span> *pa, *pb, *pqr, *pt, *ppt, *pmeas, lrot[4], trot[4];
<a name="l00183"></a>00183   <span class="comment">//int n;</span>
<a name="l00184"></a>00184   <span class="keywordtype">int</span> m, nnz;
<a name="l00185"></a>00185 
<a name="l00186"></a>00186   m=idxij-&gt;nc;
<a name="l00187"></a>00187   pa=p;
<a name="l00188"></a>00188 
<a name="l00189"></a>00189   pb = datas-&gt;points3D;
<a name="l00190"></a>00190 
<a name="l00191"></a>00191   <span class="keywordflow">for</span>(j=0; j&lt;m; ++j){
<a name="l00192"></a>00192     <span class="comment">/* j-th camera parameters */</span>
<a name="l00193"></a>00193     libmv::Mat3 rotation;
<a name="l00194"></a>00194     libmv::Mat3&amp; K = datas-&gt;intraParams[j];
<a name="l00195"></a>00195     Eigen::Quaterniond rot_init = datas-&gt;rotations[j];
<a name="l00196"></a>00196 
<a name="l00197"></a>00197     <span class="keywordtype">double</span> Kparms[] = {K( 0,0 ),K( 2,0 ),K( 2,1 ),K( 1,1 )/K( 0,0 ),K( 1,0 )};
<a name="l00198"></a>00198     <span class="comment">// full quat for initial rotation estimate:</span>
<a name="l00199"></a>00199     <span class="keywordtype">double</span> pr0[] = {rot_init.w(), rot_init.x(), rot_init.y(), rot_init.z()};
<a name="l00200"></a>00200     libmv::Mat34 proj;
<a name="l00201"></a>00201 
<a name="l00202"></a>00202     pqr=pa+j*cnp;
<a name="l00203"></a>00203     pt=pqr+3; <span class="comment">// quaternion vector part has 3 elements </span>
<a name="l00204"></a>00204     libmv::Vec3 translat = datas-&gt;translations[j];
<a name="l00205"></a>00205     <span class="keywordtype">double</span> trans[3] = {
<a name="l00206"></a>00206       pt[0] + translat(0),
<a name="l00207"></a>00207       pt[1] + translat(1),
<a name="l00208"></a>00208       pt[2] + translat(2)};
<a name="l00209"></a>00209       lrot[1]=pqr[0]; lrot[2]=pqr[1]; lrot[3]=pqr[2];
<a name="l00210"></a>00210       lrot[0]=(1.0 - lrot[1]*lrot[1] - lrot[2]*lrot[2]- lrot[3]*lrot[3]);
<a name="l00211"></a>00211       <span class="keywordflow">if</span>( lrot[0]&gt;0 )
<a name="l00212"></a>00212         lrot[0] = sqrt( lrot[0] );
<a name="l00213"></a>00213       <span class="keywordflow">else</span>{<span class="comment">//problem with this rotation...</span>
<a name="l00214"></a>00214         lrot[0] = 0;
<a name="l00215"></a>00215         Eigen::Quaterniond quat_delta( lrot[0], lrot[1], lrot[2], lrot[3] );
<a name="l00216"></a>00216         quat_delta.normalize();
<a name="l00217"></a>00217         lrot[1]=quat_delta.x(); lrot[2]=quat_delta.y(); lrot[3]=quat_delta.z();
<a name="l00218"></a>00218         lrot[0] = quat_delta.w();
<a name="l00219"></a>00219       }
<a name="l00220"></a>00220 
<a name="l00221"></a>00221       quatMultFast(lrot, pr0, trot); <span class="comment">// trot=lrot*pr0</span>
<a name="l00222"></a>00222 
<a name="l00223"></a>00223       nnz=sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); <span class="comment">/* find nonzero hx_ij, i=0...n-1 */</span>
<a name="l00224"></a>00224 
<a name="l00225"></a>00225       <span class="keywordflow">for</span>(i=0; i&lt;nnz; ++i){
<a name="l00226"></a>00226         ppt=pb + rcsubs[i]*pnp;
<a name="l00227"></a>00227         pmeas=hx + idxij-&gt;val[rcidxs[i]]*mnp; <span class="comment">// set pmeas to point to hx_ij</span>
<a name="l00228"></a>00228         calcImgProjFullR(Kparms, trot, trans, ppt, pmeas); <span class="comment">// evaluate Q in pmeas</span>
<a name="l00229"></a>00229       }
<a name="l00230"></a>00230   }
<a name="l00231"></a>00231 }
<a name="l00232"></a>00232 <span class="keywordtype">void</span> calcImgProjJacRTS(<span class="keywordtype">double</span> a[5],<span class="keywordtype">double</span> qr0[4],<span class="keywordtype">double</span> v[3],<span class="keywordtype">double</span> t[3],
<a name="l00233"></a>00233   <span class="keywordtype">double</span> M[3],<span class="keywordtype">double</span> jacmRT[2][6],<span class="keywordtype">double</span> jacmS[2][3])
<a name="l00234"></a>00234 {
<a name="l00235"></a>00235   <span class="keywordtype">double</span> t1;
<a name="l00236"></a>00236   <span class="keywordtype">double</span> t10;
<a name="l00237"></a>00237   <span class="keywordtype">double</span> t107;
<a name="l00238"></a>00238   <span class="keywordtype">double</span> t109;
<a name="l00239"></a>00239   <span class="keywordtype">double</span> t11;
<a name="l00240"></a>00240   <span class="keywordtype">double</span> t118;
<a name="l00241"></a>00241   <span class="keywordtype">double</span> t12;
<a name="l00242"></a>00242   <span class="keywordtype">double</span> t126;
<a name="l00243"></a>00243   <span class="keywordtype">double</span> t127;
<a name="l00244"></a>00244   <span class="keywordtype">double</span> t14;
<a name="l00245"></a>00245   <span class="keywordtype">double</span> t141;
<a name="l00246"></a>00246   <span class="keywordtype">double</span> t145;
<a name="l00247"></a>00247   <span class="keywordtype">double</span> t146;
<a name="l00248"></a>00248   <span class="keywordtype">double</span> t147;
<a name="l00249"></a>00249   <span class="keywordtype">double</span> t15;
<a name="l00250"></a>00250   <span class="keywordtype">double</span> t150;
<a name="l00251"></a>00251   <span class="keywordtype">double</span> t152;
<a name="l00252"></a>00252   <span class="keywordtype">double</span> t159;
<a name="l00253"></a>00253   <span class="keywordtype">double</span> t16;
<a name="l00254"></a>00254   <span class="keywordtype">double</span> t162;
<a name="l00255"></a>00255   <span class="keywordtype">double</span> t165;
<a name="l00256"></a>00256   <span class="keywordtype">double</span> t168;
<a name="l00257"></a>00257   <span class="keywordtype">double</span> t170;
<a name="l00258"></a>00258   <span class="keywordtype">double</span> t172;
<a name="l00259"></a>00259   <span class="keywordtype">double</span> t175;
<a name="l00260"></a>00260   <span class="keywordtype">double</span> t18;
<a name="l00261"></a>00261   <span class="keywordtype">double</span> t180;
<a name="l00262"></a>00262   <span class="keywordtype">double</span> t185;
<a name="l00263"></a>00263   <span class="keywordtype">double</span> t187;
<a name="l00264"></a>00264   <span class="keywordtype">double</span> t19;
<a name="l00265"></a>00265   <span class="keywordtype">double</span> t192;
<a name="l00266"></a>00266   <span class="keywordtype">double</span> t194;
<a name="l00267"></a>00267   <span class="keywordtype">double</span> t2;
<a name="l00268"></a>00268   <span class="keywordtype">double</span> t206;
<a name="l00269"></a>00269   <span class="keywordtype">double</span> t21;
<a name="l00270"></a>00270   <span class="keywordtype">double</span> t216;
<a name="l00271"></a>00271   <span class="keywordtype">double</span> t22;
<a name="l00272"></a>00272   <span class="keywordtype">double</span> t227;
<a name="l00273"></a>00273   <span class="keywordtype">double</span> t23;
<a name="l00274"></a>00274   <span class="keywordtype">double</span> t230;
<a name="l00275"></a>00275   <span class="keywordtype">double</span> t233;
<a name="l00276"></a>00276   <span class="keywordtype">double</span> t235;
<a name="l00277"></a>00277   <span class="keywordtype">double</span> t237;
<a name="l00278"></a>00278   <span class="keywordtype">double</span> t240;
<a name="l00279"></a>00279   <span class="keywordtype">double</span> t245;
<a name="l00280"></a>00280   <span class="keywordtype">double</span> t25;
<a name="l00281"></a>00281   <span class="keywordtype">double</span> t250;
<a name="l00282"></a>00282   <span class="keywordtype">double</span> t252;
<a name="l00283"></a>00283   <span class="keywordtype">double</span> t257;
<a name="l00284"></a>00284   <span class="keywordtype">double</span> t259;
<a name="l00285"></a>00285   <span class="keywordtype">double</span> t27;
<a name="l00286"></a>00286   <span class="keywordtype">double</span> t271;
<a name="l00287"></a>00287   <span class="keywordtype">double</span> t28;
<a name="l00288"></a>00288   <span class="keywordtype">double</span> t281;
<a name="l00289"></a>00289   <span class="keywordtype">double</span> t293;
<a name="l00290"></a>00290   <span class="keywordtype">double</span> t294;
<a name="l00291"></a>00291   <span class="keywordtype">double</span> t296;
<a name="l00292"></a>00292   <span class="keywordtype">double</span> t299;
<a name="l00293"></a>00293   <span class="keywordtype">double</span> t3;
<a name="l00294"></a>00294   <span class="keywordtype">double</span> t30;
<a name="l00295"></a>00295   <span class="keywordtype">double</span> t302;
<a name="l00296"></a>00296   <span class="keywordtype">double</span> t303;
<a name="l00297"></a>00297   <span class="keywordtype">double</span> t305;
<a name="l00298"></a>00298   <span class="keywordtype">double</span> t306;
<a name="l00299"></a>00299   <span class="keywordtype">double</span> t309;
<a name="l00300"></a>00300   <span class="keywordtype">double</span> t324;
<a name="l00301"></a>00301   <span class="keywordtype">double</span> t325;
<a name="l00302"></a>00302   <span class="keywordtype">double</span> t327;
<a name="l00303"></a>00303   <span class="keywordtype">double</span> t330;
<a name="l00304"></a>00304   <span class="keywordtype">double</span> t331;
<a name="l00305"></a>00305   <span class="keywordtype">double</span> t347;
<a name="l00306"></a>00306   <span class="keywordtype">double</span> t35;
<a name="l00307"></a>00307   <span class="keywordtype">double</span> t350;
<a name="l00308"></a>00308   <span class="keywordtype">double</span> t37;
<a name="l00309"></a>00309   <span class="keywordtype">double</span> t4;
<a name="l00310"></a>00310   <span class="keywordtype">double</span> t43;
<a name="l00311"></a>00311   <span class="keywordtype">double</span> t49;
<a name="l00312"></a>00312   <span class="keywordtype">double</span> t5;
<a name="l00313"></a>00313   <span class="keywordtype">double</span> t51;
<a name="l00314"></a>00314   <span class="keywordtype">double</span> t52;
<a name="l00315"></a>00315   <span class="keywordtype">double</span> t54;
<a name="l00316"></a>00316   <span class="keywordtype">double</span> t56;
<a name="l00317"></a>00317   <span class="keywordtype">double</span> t6;
<a name="l00318"></a>00318   <span class="keywordtype">double</span> t61;
<a name="l00319"></a>00319   <span class="keywordtype">double</span> t65;
<a name="l00320"></a>00320   <span class="keywordtype">double</span> t7;
<a name="l00321"></a>00321   <span class="keywordtype">double</span> t70;
<a name="l00322"></a>00322   <span class="keywordtype">double</span> t75;
<a name="l00323"></a>00323   <span class="keywordtype">double</span> t76;
<a name="l00324"></a>00324   <span class="keywordtype">double</span> t81;
<a name="l00325"></a>00325   <span class="keywordtype">double</span> t82;
<a name="l00326"></a>00326   <span class="keywordtype">double</span> t87;
<a name="l00327"></a>00327   <span class="keywordtype">double</span> t88;
<a name="l00328"></a>00328   <span class="keywordtype">double</span> t9;
<a name="l00329"></a>00329   <span class="keywordtype">double</span> t93;
<a name="l00330"></a>00330   <span class="keywordtype">double</span> t94;
<a name="l00331"></a>00331   <span class="keywordtype">double</span> t98;
<a name="l00332"></a>00332   {
<a name="l00333"></a>00333     t1 = a[0];
<a name="l00334"></a>00334     t2 = v[0];
<a name="l00335"></a>00335     t3 = t2*t2;
<a name="l00336"></a>00336     t4 = v[1];
<a name="l00337"></a>00337     t5 = t4*t4;
<a name="l00338"></a>00338     t6 = v[2];
<a name="l00339"></a>00339     t7 = t6*t6;
<a name="l00340"></a>00340     t9 = sqrt(1.0-t3-t5-t7);
<a name="l00341"></a>00341     t10 = 1/t9;
<a name="l00342"></a>00342     t11 = qr0[1];
<a name="l00343"></a>00343     t12 = t11*t10;
<a name="l00344"></a>00344     t14 = qr0[0];
<a name="l00345"></a>00345     t15 = -t12*t2+t14;
<a name="l00346"></a>00346     t16 = M[0];
<a name="l00347"></a>00347     t18 = qr0[2];
<a name="l00348"></a>00348     t19 = t18*t10;
<a name="l00349"></a>00349     t21 = qr0[3];
<a name="l00350"></a>00350     t22 = -t19*t2-t21;
<a name="l00351"></a>00351     t23 = M[1];
<a name="l00352"></a>00352     t25 = t10*t21;
<a name="l00353"></a>00353     t27 = -t25*t2+t18;
<a name="l00354"></a>00354     t28 = M[2];
<a name="l00355"></a>00355     t30 = -t15*t16-t22*t23-t27*t28;
<a name="l00356"></a>00356     t35 = -t9*t11-t2*t14-t4*t21+t6*t18;
<a name="l00357"></a>00357     t37 = -t35;
<a name="l00358"></a>00358     t43 = t9*t18+t4*t14+t6*t11-t2*t21;
<a name="l00359"></a>00359     t49 = t9*t21+t6*t14+t2*t18-t11*t4;
<a name="l00360"></a>00360     t51 = -t37*t16-t43*t23-t49*t28;
<a name="l00361"></a>00361     t52 = -t15;
<a name="l00362"></a>00362     t54 = t10*t14;
<a name="l00363"></a>00363     t56 = -t54*t2-t11;
<a name="l00364"></a>00364     t61 = t9*t14-t2*t11-t4*t18-t6*t21;
<a name="l00365"></a>00365     t65 = t61*t16+t43*t28-t23*t49;
<a name="l00366"></a>00366     t70 = t56*t16+t22*t28-t23*t27;
<a name="l00367"></a>00367     t75 = t56*t23+t27*t16-t28*t15;
<a name="l00368"></a>00368     t76 = -t49;
<a name="l00369"></a>00369     t81 = t61*t23+t49*t16-t37*t28;
<a name="l00370"></a>00370     t82 = -t27;
<a name="l00371"></a>00371     t87 = t56*t28+t23*t15-t22*t16;
<a name="l00372"></a>00372     t88 = -t43;
<a name="l00373"></a>00373     t93 = t61*t28+t37*t23-t43*t16;
<a name="l00374"></a>00374     t94 = -t22;
<a name="l00375"></a>00375     t98 = a[4];
<a name="l00376"></a>00376     t107 = t30*t88+t94*t51+t56*t81+t61*t75+t87*t35+t93*t52-t70*t76-t82*t65;
<a name="l00377"></a>00377     t109 = a[1];
<a name="l00378"></a>00378     t118 = t30*t76+t82*t51+t56*t93+t61*t87+t70*t88+t65*t94-t35*t75-t81*t52;
<a name="l00379"></a>00379     t126 = t76*t51+t61*t93+t65*t88-t81*t35+t[2];
<a name="l00380"></a>00380     t127 = 1/t126;
<a name="l00381"></a>00381     t141 = t51*t88+t61*t81+t93*t35-t65*t76+t[1];
<a name="l00382"></a>00382     t145 = t126*t126;
<a name="l00383"></a>00383     t146 = 1/t145;
<a name="l00384"></a>00384     t147 = (t1*(t35*t51+t61*t65+t81*t76-t93*t88+t[0])+t98*t141+t126*t109)*t146;
<a name="l00385"></a>00385     jacmRT[0][0] = (t1*(t30*t35+t52*t51+t56*t65+t61*t70+t76*t75+t81*t82-t88*t87
<a name="l00386"></a>00386       -t93*t94)+t98*t107+t109*t118)*t127-t118*t147;
<a name="l00387"></a>00387     t150 = t1*a[3];
<a name="l00388"></a>00388     t152 = a[2];
<a name="l00389"></a>00389     t159 = (t150*t141+t126*t152)*t146;
<a name="l00390"></a>00390     jacmRT[1][0] = (t107*t150+t152*t118)*t127-t159*t118;
<a name="l00391"></a>00391     t162 = -t12*t4+t21;
<a name="l00392"></a>00392     t165 = -t19*t4+t14;
<a name="l00393"></a>00393     t168 = -t25*t4-t11;
<a name="l00394"></a>00394     t170 = -t162*t16-t165*t23-t168*t28;
<a name="l00395"></a>00395     t172 = -t162;
<a name="l00396"></a>00396     t175 = -t54*t4-t18;
<a name="l00397"></a>00397     t180 = t175*t16+t165*t28-t168*t23;
<a name="l00398"></a>00398     t185 = t175*t23+t168*t16-t162*t28;
<a name="l00399"></a>00399     t187 = -t168;
<a name="l00400"></a>00400     t192 = t175*t28+t162*t23-t165*t16;
<a name="l00401"></a>00401     t194 = -t165;
<a name="l00402"></a>00402     t206 = t170*t88+t51*t194+t175*t81+t61*t185+t192*t35+t93*t172-t76*t180-t65*
<a name="l00403"></a>00403       t187;
<a name="l00404"></a>00404     t216 = t170*t76+t51*t187+t93*t175+t61*t192+t180*t88+t65*t194-t185*t35-t81*
<a name="l00405"></a>00405       t172;
<a name="l00406"></a>00406     jacmRT[0][1] = (t1*(t170*t35+t172*t51+t175*t65+t180*t61+t185*t76+t81*t187-
<a name="l00407"></a>00407       t192*t88-t93*t194)+t98*t206+t109*t216)*t127-t147*t216;
<a name="l00408"></a>00408     jacmRT[1][1] = (t150*t206+t152*t216)*t127-t159*t216;
<a name="l00409"></a>00409     t227 = -t12*t6-t18;
<a name="l00410"></a>00410     t230 = -t19*t6+t11;
<a name="l00411"></a>00411     t233 = -t25*t6+t14;
<a name="l00412"></a>00412     t235 = -t227*t16-t23*t230-t233*t28;
<a name="l00413"></a>00413     t237 = -t227;
<a name="l00414"></a>00414     t240 = -t54*t6-t21;
<a name="l00415"></a>00415     t245 = t240*t16+t230*t28-t233*t23;
<a name="l00416"></a>00416     t250 = t23*t240+t233*t16-t227*t28;
<a name="l00417"></a>00417     t252 = -t233;
<a name="l00418"></a>00418     t257 = t240*t28+t227*t23-t230*t16;
<a name="l00419"></a>00419     t259 = -t230;
<a name="l00420"></a>00420     t271 = t235*t88+t51*t259+t81*t240+t61*t250+t257*t35+t93*t237-t245*t76-t65*
<a name="l00421"></a>00421       t252;
<a name="l00422"></a>00422     t281 = t235*t76+t51*t252+t240*t93+t61*t257+t245*t88+t259*t65-t250*t35-t81*
<a name="l00423"></a>00423       t237;
<a name="l00424"></a>00424     jacmRT[0][2] = (t1*(t235*t35+t237*t51+t240*t65+t61*t245+t250*t76+t81*t252-
<a name="l00425"></a>00425       t257*t88-t93*t259)+t271*t98+t281*t109)*t127-t147*t281;
<a name="l00426"></a>00426     jacmRT[1][2] = (t150*t271+t281*t152)*t127-t159*t281;
<a name="l00427"></a>00427     jacmRT[0][3] = t127*t1;
<a name="l00428"></a>00428     jacmRT[1][3] = 0.0;
<a name="l00429"></a>00429     jacmRT[0][4] = t98*t127;
<a name="l00430"></a>00430     jacmRT[1][4] = t150*t127;
<a name="l00431"></a>00431     jacmRT[0][5] = t109*t127-t147;
<a name="l00432"></a>00432     jacmRT[1][5] = t152*t127-t159;
<a name="l00433"></a>00433     t293 = t35*t35;
<a name="l00434"></a>00434     t294 = t61*t61;
<a name="l00435"></a>00435     t296 = t88*t88;
<a name="l00436"></a>00436     t299 = t35*t88;
<a name="l00437"></a>00437     t302 = t61*t76;
<a name="l00438"></a>00438     t303 = 2.0*t299+t61*t49-t302;
<a name="l00439"></a>00439     t305 = t35*t76;
<a name="l00440"></a>00440     t306 = t61*t88;
<a name="l00441"></a>00441     t309 = t305+2.0*t306-t49*t35;
<a name="l00442"></a>00442     jacmS[0][0] = (t1*(t293+t294+t49*t76-t296)+t98*t303+t109*t309)*t127-t147*
<a name="l00443"></a>00443       t309;
<a name="l00444"></a>00444     jacmS[1][0] = (t150*t303+t152*t309)*t127-t159*t309;
<a name="l00445"></a>00445     t324 = t76*t76;
<a name="l00446"></a>00446     t325 = t296+t294+t35*t37-t324;
<a name="l00447"></a>00447     t327 = t76*t88;
<a name="l00448"></a>00448     t330 = t61*t35;
<a name="l00449"></a>00449     t331 = 2.0*t327+t61*t37-t330;
<a name="l00450"></a>00450     jacmS[0][1] = (t1*(t299+2.0*t302-t37*t88)+t98*t325+t109*t331)*t127-t147*
<a name="l00451"></a>00451       t331;
<a name="l00452"></a>00452     jacmS[1][1] = (t150*t325+t152*t331)*t127-t159*t331;
<a name="l00453"></a>00453     t347 = t327+2.0*t330-t43*t76;
<a name="l00454"></a>00454     t350 = t324+t294+t43*t88-t293;
<a name="l00455"></a>00455     jacmS[0][2] = (t1*(2.0*t305+t61*t43-t306)+t98*t347+t350*t109)*t127-t147*
<a name="l00456"></a>00456       t350;
<a name="l00457"></a>00457     jacmS[1][2] = (t150*t347+t152*t350)*t127-t159*t350;
<a name="l00458"></a>00458     <span class="keywordflow">return</span>;
<a name="l00459"></a>00459   }
<a name="l00460"></a>00460 }
<a name="l00461"></a>00461 
<a name="l00462"></a>00462 <span class="keyword">static</span> <span class="keywordtype">void</span> img_projsRTS_jac_x(<span class="keywordtype">double</span> *p, <span class="keyword">struct</span> sba_crsm *idxij, <span class="keywordtype">int</span> *rcidxs, <span class="keywordtype">int</span> *rcsubs, <span class="keywordtype">double</span> *jac, <span class="keywordtype">void</span> *adata)
<a name="l00463"></a>00463 {
<a name="l00464"></a>00464   bundle_datas* datas = ((bundle_datas*)adata);
<a name="l00465"></a>00465   <span class="keyword">register</span> <span class="keywordtype">int</span> i, j;
<a name="l00466"></a>00466   <span class="keywordtype">int</span> cnp = datas-&gt;cnp, pnp = datas-&gt;pnp, mnp = datas-&gt;mnp;
<a name="l00467"></a>00467 
<a name="l00468"></a>00468   <span class="keywordtype">double</span> *pa, *pb, *pqr, *pt, *ppt, *pA, *pB;
<a name="l00469"></a>00469   <span class="comment">//int n;</span>
<a name="l00470"></a>00470   <span class="keywordtype">int</span> m, nnz, Asz, Bsz, ABsz;
<a name="l00471"></a>00471 
<a name="l00472"></a>00472   <span class="comment">//n=idxij-&gt;nr;</span>
<a name="l00473"></a>00473   m=idxij-&gt;nc;
<a name="l00474"></a>00474   pa=p; pb=p+m*cnp;
<a name="l00475"></a>00475   Asz=mnp*cnp; Bsz=mnp*pnp; ABsz=Asz+Bsz;
<a name="l00476"></a>00476 
<a name="l00477"></a>00477   <span class="keywordflow">for</span>(j=0; j&lt;m; ++j){
<a name="l00478"></a>00478     <span class="comment">/* j-th camera parameters */</span>
<a name="l00479"></a>00479     pqr=pa+j*cnp;
<a name="l00480"></a>00480     pt=pqr+3; <span class="comment">// quaternion vector part has 3 elements</span>
<a name="l00481"></a>00481     libmv::Vec3 translat = datas-&gt;translations[j];
<a name="l00482"></a>00482     <span class="keywordtype">double</span> vec_translat[3] = {
<a name="l00483"></a>00483       pt[0] + translat(0),
<a name="l00484"></a>00484       pt[1] + translat(1),
<a name="l00485"></a>00485       pt[2] + translat(2)};
<a name="l00486"></a>00486       libmv::Mat3 rotation;
<a name="l00487"></a>00487       libmv::Mat3&amp; K = datas-&gt;intraParams[j];
<a name="l00488"></a>00488       Eigen::Quaterniond rot_init = datas-&gt;rotations[j];
<a name="l00489"></a>00489 
<a name="l00490"></a>00490       <span class="keywordtype">double</span> Kparms[] = {K( 0,0 ),K( 2,0 ),K( 2,1 ),K( 1,1 )/K( 0,0 ),K( 1,0 )};
<a name="l00491"></a>00491       <span class="comment">// full quat for initial rotation estimate:</span>
<a name="l00492"></a>00492       <span class="keywordtype">double</span> pr0[] = {rot_init.w(), rot_init.x(), rot_init.y(), rot_init.z()};
<a name="l00493"></a>00493       libmv::Mat34 proj;
<a name="l00494"></a>00494 
<a name="l00495"></a>00495       nnz=sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); <span class="comment">/* find nonzero hx_ij, i=0...n-1 */</span>
<a name="l00496"></a>00496 
<a name="l00497"></a>00497       <span class="keywordflow">for</span>(i=0; i&lt;nnz; ++i){
<a name="l00498"></a>00498         ppt=pb + rcsubs[i]*pnp;
<a name="l00499"></a>00499         pA=jac + idxij-&gt;val[rcidxs[i]]*ABsz; <span class="comment">// set pA to point to A_ij</span>
<a name="l00500"></a>00500         pB=pA  + Asz; <span class="comment">// set pB to point to B_ij</span>
<a name="l00501"></a>00501 
<a name="l00502"></a>00502         calcImgProjJacRTS(Kparms, pr0, pqr, vec_translat, ppt, (<span class="keywordtype">double</span> (*)[6])pA, (<span class="keywordtype">double</span> (*)[3])pB); <span class="comment">// evaluate dQ/da, dQ/db in pA, pB</span>
<a name="l00503"></a>00503       }
<a name="l00504"></a>00504   }
<a name="l00505"></a>00505 }
<a name="l00506"></a>00506 
<a name="l00507"></a>00507 }
<a name="l00508"></a>00508 
<a name="l00509"></a>00509 <span class="preprocessor">#endif</span>
</pre></div></div>
</div>
<!-- window showing the filter options -->
<div id="MSearchSelectWindow"
     onmouseover="return searchBox.OnSearchSelectShow()"
     onmouseout="return searchBox.OnSearchSelectHide()"
     onkeydown="return searchBox.OnSearchSelectKey(event)">
<a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(0)"><span class="SelectionMark">&#160;</span>All</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(1)"><span class="SelectionMark">&#160;</span>Classes</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(2)"><span class="SelectionMark">&#160;</span>Functions</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(3)"><span class="SelectionMark">&#160;</span>Variables</a></div>

<!-- iframe showing the search results (closed by default) -->
<div id="MSearchResultsWindow">
<iframe src="javascript:void(0)" frameborder="0" 
        name="MSearchResults" id="MSearchResults">
</iframe>
</div>



<hr class="footer"/><address class="footer"><small>
Generated on Sun Aug 21 2011 16:45:52 for GSoC2011SfM by &#160;<a href="http://www.doxygen.org/index.html">
<img class="footer" src="doxygen.png" alt="doxygen"/>
</a> 1.7.5.1
</small></address>

</body>
</html>
